//=============================================================================
//     _   _                               ____                _  _          
//    | | | |  __ _  _ __   _ __   _   _  / ___|  _ __   _ __ (_)| |_   ___ 
//    | |_| | / _` || '_ \ | '_ \ | | | | \___ \ | '_ \ | '__|| || __| / _ \
//    |  _  || (_| || |_) || |_) || |_| |  ___) || |_) || |   | || |_ |  __/
//    |_| |_| \__,_|| .__/ | .__/  \__, | |____/ | .__/ |_|   |_| \__| \___|
//                  |_|    |_|     |___/         |_|                         
//
//                     HappySprite - We make sprites happy
//
// Copyright (c) 2007 by Tank Monkey Games
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
//=============================================================================

using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace HappySprite.Simulation
{
    internal static class ConvexConvexAlgorithm
    {
        static GjkSimplexSolver gjk = new GjkSimplexSolver();
        static PenetrationDepthSolver mtd = new PenetrationDepthSolver();

        static Transform2D worldP;
        static Transform2D worldQ;
        static CollisionShape thisShape;
        static CollisionShape otherShape;

        internal static bool DetectCollision(CollisionEventArgs e)
        {
            // Initialize globals
            thisShape = e.This.CollisionShape;
            otherShape = e.Other.CollisionShape;
            worldP = e.This.Transform;
            worldQ = e.Other.Transform;

            // False means no collision
            if (!GjkSeparatingAxisAlgorithm())
                return false;

            if (e.CollisionDetails == CollisionDetails.None)
                return true;

            // Degenerative simplex (a line), assume this is a circle-circle collision.
            if (!gjk.FullSimplex)
            {
                e.Depth = float.PositiveInfinity;

                foreach (Vector2 point in gjk.SimplexPoints())
                {
                    float d = point.Length();

                    if (d < e.Depth)
                        e.Depth = d;
                }

                e.Normal = Vector2.Normalize(worldQ.Position - worldP.Position);
            }
            else
            {
                // Find normal, depth, and contact point
                PenetrationDepthAlgorithm(out e.Normal, out e.Depth);
            }

            Vector2 pContactPoint, qContactPoint;
            gjk.ComputeClosestPoints(out pContactPoint, out qContactPoint);

            e.ContactPoint = pContactPoint;

            return true;
        }

        // An implementation of the incremental separating-axis GJK algorithm (ISA-GJK)
        static bool GjkSeparatingAxisAlgorithm()
        {
            // Initialize
            Vector2 seperatingAxis = Vector2.UnitY;
            float squaredDistance = float.MaxValue;
            float error = 0f;

            // Reset the simplex singleton
            gjk.Reset();

            do
            {
                // Get a support point on the Minkowski sum of P - Q.
                Vector2 pWorld = thisShape.SupportMapping(worldP, -seperatingAxis);
                Vector2 qWorld = otherShape.SupportMapping(worldQ, seperatingAxis);
                Vector2 newPoint = pWorld - qWorld;

                // Exit if we found a seperating axis of P and Q (no collision).
                float delta = Vector2.Dot(seperatingAxis, newPoint);
                if (delta > 0f) return false;

                // Compute the new smallest simplex containing the support point.
                gjk.AddSupportPoint(newPoint, pWorld, qWorld);

                float previousSquaredDistance = squaredDistance;
                seperatingAxis = gjk.ClosestPoint;
                squaredDistance = seperatingAxis.LengthSquared();

                // Check for degenerative case (algorithm will never terminate).
                if ((previousSquaredDistance - squaredDistance) <= (1e-05f * previousSquaredDistance))
                    return false;

                error = 3e-05f * gjk.MaxLengthSquared;
            }
            while (!gjk.FullSimplex && (squaredDistance >= error));

            // Closest point on the simplex to origin is close enough.
            return true;
        }

        // Implementation of the expanding polytope algorithm (GJK-EPA).
        static void PenetrationDepthAlgorithm(out Vector2 normal, out float depth)
        {
            // Initialize the solver.
            mtd.Initialize(gjk.SimplexPoints());

            Vector2 v = mtd.MinimumPenetration;
            float squaredDistance = mtd.MinimumPenetration.LengthSquared();
            float error = 0f;

            // Degenerative starting case.
            if (squaredDistance < 1e-10f)
                v = new Vector2(-1f, -1f);

            do
            {
                //Vector2 v = mtd.MinimumPenetration;

                //if (squaredDistance < 1e-10f)
                //    v = new Vector2(-1f, -1f);

                // Get a support point on the Minkowski sum of P - Q.
                Vector2 pWorld = thisShape.SupportMapping(worldP, v);
                Vector2 qWorld = otherShape.SupportMapping(worldQ, -v);
                Vector2 newPoint = pWorld - qWorld;

                mtd.AddSupportPoint(newPoint);

                float previousSquaredDistance = squaredDistance;
                v = mtd.MinimumPenetration;
                squaredDistance = v.LengthSquared();

                // Check for infinite loop (we are not progressing towards a solution).
                if ((previousSquaredDistance - squaredDistance) <= (1e-05f * previousSquaredDistance))
                    break;

                error = 3e-05f * mtd.MaxLengthSquared;
            }
            while (squaredDistance >= error);

            normal = mtd.MinimumPenetration;
            depth = normal.Length();

            if (depth < 1e-10f)
                normal = mtd.EdgeNormal;
            else
                normal = Vector2.Normalize(normal);
        }

        //struct ContactPointNode : IComparable<ContactPointNode>
        //{
        //    public float Det;
        //    public Vector2 Point;

        //    public ContactPointNode(float det, Vector2 point)
        //    {
        //        Det = det;
        //        Point = point;
        //    }

        //    int IComparable<ContactPointNode>.CompareTo(ContactPointNode a)
        //    {
        //        return Det.CompareTo(a.Det);
        //    }
        //}

        //static Feature ComputeContactPoints(Vector2 normal)
        //{
        //    Feature featureP = thisShape.ClosestFeature(Vector2.TransformNormal(normal, transposeP));
        //    Feature featureQ = otherShape.ClosestFeature(Vector2.TransformNormal(-normal, transposeQ));

        //    Feature contactPoints = new Feature();
        //    contactPoints.FeatureType = FeatureType.Point;

        //    if (featureP.FeatureType == FeatureType.Point)
        //        contactPoints.PointOne = Vector2.Transform(featureP.PointOne, worldP);
        //    else if (featureQ.FeatureType == FeatureType.Point)
        //    {
        //        featureP.PointOne = Vector2.Transform(featureP.PointOne, worldP);
        //        featureP.PointTwo = Vector2.Transform(featureP.PointTwo, worldP);
        //        contactPoints.PointOne = ProjectPointOntoEdge(Vector2.Transform(featureQ.PointOne, worldQ), featureP.PointOne, featureP.PointTwo);
        //    }
        //    else
        //    {
        //        featureP.PointOne = Vector2.Transform(featureP.PointOne, worldP);
        //        featureP.PointTwo = Vector2.Transform(featureP.PointTwo, worldP);

        //        PriorityQueue<ContactPointNode> points = new PriorityQueue<ContactPointNode>();
        //        Vector2 perp = new Vector2(normal.Y, -normal.X);
        //        ContactPointNode node;
        //        Vector2 v;
        //        float dot;

        //        v = featureP.PointOne;
        //        dot = Vector2.Dot(v, perp);
        //        node = new ContactPointNode(dot, v);
        //        points.Push(node);

        //        v = featureP.PointTwo;
        //        dot = Vector2.Dot(v, perp);
        //        node = new ContactPointNode(dot, v);
        //        points.Push(node);

        //        v = Vector2.Transform(featureQ.PointOne, worldQ);
        //        dot = Vector2.Dot(v, perp);
        //        node = new ContactPointNode(dot, v);
        //        points.Push(node);

        //        v = Vector2.Transform(featureQ.PointTwo, worldQ);
        //        dot = Vector2.Dot(v, perp);
        //        node = new ContactPointNode(dot, v);
        //        points.Push(node);

        //        points.Pop();
        //        contactPoints.PointOne = ProjectPointOntoEdge(points.Pop().Point, featureP.PointOne, featureP.PointTwo);
        //        contactPoints.PointTwo = ProjectPointOntoEdge(points.Pop().Point, featureP.PointOne, featureP.PointTwo);

        //        if (contactPoints.PointOne != contactPoints.PointTwo)
        //            contactPoints.FeatureType = FeatureType.Edge;
        //    }

        //    return contactPoints;
        //}

        //static Vector2 ProjectPointOntoEdge(Vector2 point, Vector2 edgeStart, Vector2 edgeEnd)
        //{
        //    Vector2 v = edgeEnd - edgeStart;
        //    Vector2 w = point - edgeStart;

        //    float c1 = Vector2.Dot(w, v);
        //    float c2 = Vector2.Dot(v, v);
        //    float b = c1 / c2;

        //    return b * v + edgeStart;
        //}
    }
}
